#include "formation_avoidance/Astar.h"

double Astar::getDiagHeu(GridMapNode* node1, GridMapNode* node2)
{
    double dx = abs(node1->index(0) - node2->index(0));
    double dy = abs(node1->index(1) - node2->index(1));
    double dz = abs(node1->index(2) - node2->index(2));

    double h = 0.0;
    int diag = std::min(std::min(dx, dy), dz);
    dx -= diag;
    dy -= diag;
    dz -= diag;

    if (dx == 0)
    {
        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * std::min(dy, dz) + 1.0 * abs(dy - dz);
    }
    if (dy == 0)
    {
        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * std::min(dx, dz) + 1.0 * abs(dx - dz);
    }
    if (dz == 0)
    {
        h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * std::min(dx, dy) + 1.0 * abs(dx - dy);
    }
    return h;
}

void Astar::initGridMap()
{
    Eigen::Vector3i POOL_SIZE_(pool_size,pool_size,pool_size_z);

    GridMap_ = new GridMapNodePtr **[POOL_SIZE_(0)];
    for (int i = 0; i < POOL_SIZE_(0); i++)
    {
        GridMap_[i] = new GridMapNodePtr *[POOL_SIZE_(1)];
        for (int j = 0; j < POOL_SIZE_(1); j++)
        {
            GridMap_[i][j] = new GridMapNodePtr[POOL_SIZE_(2)];
            for (int k = 0; k < POOL_SIZE_(2); k++)
            {
                GridMap_[i][j][k] = new GridMapNode;
            }
        }
    }
}

void Astar::AstarLinkMap(gvm::GMMVoxelMapCircularBuffer* gvmMap,double set_local_length,int set_agent_id)
{
	AstarMapPtr=gvmMap;
	local_length=set_local_length;
	agent_id=set_agent_id;
	color_id=agent_id+32;
	initGridMap();
	cube_inflater.GridCubeLinkMap(gvmMap);
}


void Astar::a_star_retrievePath(GridMapNode* final_node)
{
    a_star_gridpath.clear();
    a_star_gridpath.push_back(final_node->index);

    while (final_node->parent_node != NULL)
    {
        final_node = final_node->parent_node ;
        a_star_gridpath.push_back(final_node->index);
    }
	std::reverse(a_star_gridpath.begin(),a_star_gridpath.end());

}

double Astar::getpointdistance(Eigen::Vector3i idx1, Eigen::Vector3i idx2)
{
	double tmp=(idx1-idx2).cast<double>().norm();
    return tmp;
}

void Astar::a_star_bresenham3D(double bresenham3DSafetyDistance)
{
    std::vector<Eigen::Vector3i> tmp_path=a_star_gridpath;
    a_star_gridpath.clear();
    a_star_gridpath.push_back(*tmp_path.begin());

    Eigen::Vector3i bresenham3Dstart_idx=*tmp_path.begin();

    int j=0;
    for(j=1;j<(int)tmp_path.size();j++)
    {
        std::vector<Eigen::Vector3i> bresenham3Dpath=getbresenham3D(bresenham3Dstart_idx, tmp_path[j]);
        for(int i=0;i<(int)bresenham3Dpath.size();i++)
        {
            int tmp=i*1;
            if(tmp>=(int)bresenham3Dpath.size()) break;
            if(AstarMapPtr->getMapLocalIdxEdt(bresenham3Dpath[i])<bresenham3DSafetyDistance)
            {
                a_star_gridpath.push_back(tmp_path[j-1]);
                bresenham3Dstart_idx=tmp_path[j-1];
                break;
            }
        }
    }
    a_star_gridpath.push_back(*(tmp_path.end()-1));
    // ROS_INFO("a_star_grid_path size after bresenham3D: %ld",a_star_gridpath.size());
}


void Astar::a_star_RemoveCollinearPath()
{
    std::vector<Eigen::Vector3i> tmp_path=a_star_gridpath;
    a_star_gridpath.clear();

    a_star_gridpath.push_back(*tmp_path.begin());

    for (std::vector<Eigen::Vector3i>::const_iterator iter = tmp_path.begin(); iter != tmp_path.end()-2; iter++)
    {
        // ROS_INFO("SSSSS");
        // GridMapNode* node1=*(iter+2);
        // std::cout<<"*(iter+2)->index(): "<<node1->index<<std::endl;
        double distance13 = getpointdistance(*(iter+2), *iter);
        double distance12 = getpointdistance(*(iter+1), *iter);
        double distance23 = getpointdistance(*(iter+2),*(iter+1));
        if (distance13==distance12 + distance23||distance12==distance13 + distance23||distance23==distance13 + distance12) 
        {
            // ROS_INFO("distance %f %f %f",distance13,distance12,distance23);
			// ROS_INFO("node 1 : %d %d %d",(*iter)->index(0),(*iter)->index(1),(*iter)->index(2));
			// ROS_INFO("node 2 : %d %d %d",(*(iter+1))->index(0),(*(iter+1))->index(1),(*(iter+1))->index(2));
			// ROS_INFO("node 3 : %d %d %d",(*(iter+2))->index(0),(*(iter+2))->index(1),(*(iter+2))->index(2));

            continue;
        }
        a_star_gridpath.push_back(*(iter+1));
    }
    a_star_gridpath.push_back(*(tmp_path.end()-1));

    // ROS_INFO("a_star_grid_path size after remove collinear point: %ld",a_star_gridpath.size());
}


bool Astar::corridor_generation(double CorridorGenerationSafetyDistance)
{
    CorridorCube.clear();
    Eigen::MatrixXi last_InflateCube=Eigen::MatrixXi::Zero(8,3);
    for(int i=0;i<(int)a_star_gridpath.size();i++)
    {
        Eigen::Vector3i index=a_star_gridpath[i];
		cube_inflater.setindex(index);
        if(cube_inflater.InflateCube(last_InflateCube,CorridorGenerationSafetyDistance))
        {
            cube_inflater.UpdateVertexPos();
            CorridorCube.push_back(cube_inflater);
            last_InflateCube=cube_inflater.vertex_idx;
        }
    }
    // visual_corridor();
    // ROS_INFO("CorridorCube.size(): %ld",CorridorCube.size());
    return true;
}


bool Astar::a_star_search(Eigen::Vector3d start_p,Eigen::Vector3d end_p,double AstarSafetyDistance,Eigen::Vector3d& local_goal,
						std::vector<Eigen::Vector3i>& a_star_path )
{
	a_star_path.clear();
	a_star_gridpath.clear();

    ros::Time time_1 = ros::Time::now();

    rounds_++;
	std::priority_queue<GridMapNode*, std::vector<GridMapNode*>, NodeComparator> empty;
    std::priority_queue<GridMapNode*, std::vector<GridMapNode*>, NodeComparator> openSet_;

    openSet_.swap(empty);

	Eigen::Vector3i start_index;
	Eigen::Vector3i end_index;

    // rrb.get_distance_idx(start_p,start_index);
	AstarMapPtr->getmapIdxLocal(start_p,start_index);


	// std::cout<<"start_index: \n"<< start_index<<std::endl;
    // ROS_INFO("start_index->index: %d %d %d ",start_index(0),start_index(1),start_index(2));


    // rrb.get_distance_idx(end_p,end_index);
	AstarMapPtr->getmapIdxLocal(end_p,end_index);

	// std::cout<<"end_index: \n"<< end_index<<std::endl;
    // ROS_INFO("end_index->index: %d %d %d ",end_index(0),end_index(1),end_index(2));


    // return true;
    // ROS_INFO("ssssssssss");
    Eigen::Vector3i centre(pool_size/2,pool_size/2,pool_size_z/2);
    Eigen::Vector3i offset=start_index-centre;

	GridMapNode* start_node=GridMap_[pool_size/2][pool_size/2][pool_size_z/2];

    GridMapNode* end_node=new GridMapNode;
	start_node->index=start_index;
	end_node->index=end_index;

	// Eigen::Vector3i test_idx=start_index;
	// test_idx(0)+=70;
	// double test_ids=AstarMapPtr->getMapLocalIdxEdt(test_idx);
	// ROS_INFO("a star test DIS: %f",test_ids);

    if( AstarMapPtr->getMapLocalIdxEdt(start_index)<AstarSafetyDistance)
    {
        // ROS_WARN("start point is unsafe");
        // return false;
    }
    if(AstarMapPtr->getMapLocalIdxEdt(end_index)<AstarSafetyDistance)
    {
        // ROS_WARN("end point is unsafe");
        // return false;
    }

    start_node->rounds = rounds_;
    start_node->g_score = 0;
    // ROS_INFO("bbbbbbbbbbbbbbb");

    start_node->f_score = getHeu(start_node, end_node);
    // ROS_INFO("AAAAAAAAAAAAA");
    start_node->state = GridMapNode::OPENSET; //put start node in open set
    start_node->parent_node = NULL;
	
    openSet_.push(start_node); //put start in open set
    int num_iter=0;

    std::vector<Eigen::Vector3i> closeSet_idx;
    GridMapNode* current_node;
    GridMapNode* neighborPtr;

    // ROS_INFO("GO INTO A STAR LOOP");
	Eigen::Vector3i local_goal_idx;
	while(!openSet_.empty())
	{
        num_iter++;
		current_node=openSet_.top();
        openSet_.pop();
        // ROS_INFO("///////////current_node->index: %d %d %d ",current_node->index(0),current_node->index(1),current_node->index(2));

        Eigen::Vector3d current_node_position=AstarMapPtr->getMapGlobalPosfromLocalIdx(current_node->index);

//        if (current_node->index == end_node->index||(start_p-current_node_position).norm()>=local_length)
        if (current_node->index == end_node->index||(start_p-current_node_position).norm()>=local_length)
        {
            // ros::Time time_2 = ros::Time::now();
            // printf("\033[34mA star iter:%d, time:%.3d\033[0m\n",num_iter, (time_2 - time_1).toSec()*1000);
            // if((time_2 - time_1).toSec() > 0.1)
            //     ROS_WARN("Time consume in A star path finding is %f", (time_2 - time_1).toSec() );
            // ROS_INFO("A STAR FIND PATH");
            a_star_retrievePath(current_node);
            a_star_RemoveCollinearPath();
            a_star_bresenham3D(AstarSafetyDistance);
            corridor_generation(AstarSafetyDistance);
			a_star_path=a_star_gridpath;
            local_goal=current_node_position;
			local_goal_idx=current_node->index;
            break;
        }

        current_node->state = GridMapNode::CLOSEDSET; //move current node from open set to closed set.
        closeSet_idx.push_back(current_node->index);
		for (int dx = -1; dx <= 1; dx++)
        {
            for (int dy = -1; dy <= 1; dy++)
            {
					int dz=0;
                    if (dx == 0 && dy == 0 && dz == 0) continue;
                    Eigen::Vector3i neighborIdx;
                    neighborIdx(0) = (current_node->index)(0) + dx;
                    neighborIdx(1) = (current_node->index)(1) + dy;
                    neighborIdx(2) = (current_node->index)(2) + dz;

                    Eigen::Vector3i ConvertNeighborIdx=neighborIdx-offset;
                    if(ConvertNeighborIdx(0)>=pool_size||ConvertNeighborIdx(1)>=pool_size||ConvertNeighborIdx(2)>=pool_size_z
                        || ConvertNeighborIdx(0)<=0||ConvertNeighborIdx(1)<=0||ConvertNeighborIdx(2)<=0||neighborIdx(2)<=3)
                        continue;

                    neighborPtr=GridMap_[ConvertNeighborIdx(0)][ConvertNeighborIdx(1)][ConvertNeighborIdx(2)];
                    neighborPtr->index = neighborIdx;
 
                    bool flag_explored = neighborPtr->rounds == rounds_;

                    if (flag_explored && neighborPtr->state == GridMapNode::CLOSEDSET)
                    {
                        // ROS_INFO("SSASASASASAS");
                        continue; //in closed set.
                    }
                    neighborPtr->rounds = rounds_;

                    // if (neighborPtr->checkOccupancy())
                    if( AstarMapPtr->getMapLocalIdxEdt(neighborPtr->index )<AstarSafetyDistance)
                    {
                        // ROS_INFO("555555555");
                        // ROS_INFO("danger: neighborPtr->index: %d %d %d ",neighborIdx(0),neighborIdx(1),neighborIdx(2));
                        continue;
                    }

                    // Eigen::Vector3d path_point=AstarMapPtr->getMapGlobalPosfromLocalIdx(neighborPtr->index);
                    // if(path_point(2)>2.0)
                    //     continue;

                    double static_cost=sqrt(dx * dx + dy * dy + dz * dz);
                    double tentative_gScore = current_node->g_score + static_cost;

                    if (!flag_explored)
                    {
                        //discover a new node
                        neighborPtr->state = GridMapNode::OPENSET;
                        neighborPtr->parent_node = current_node;
                        neighborPtr->g_score = tentative_gScore;
                        neighborPtr->f_score = tentative_gScore + getHeu(neighborPtr, end_node);
                        openSet_.push(neighborPtr); //put neighbor in open set and record it.
                        // ROS_INFO("neighborPtr->index: %d %d %d",neighborPtr->index(0),neighborPtr->index(1),neighborPtr->index(2));
                        if(neighborPtr->index==current_node->index)return true;

                    }
                    else if (tentative_gScore < neighborPtr->g_score)
                    { 
                        neighborPtr->parent_node = current_node;
                        neighborPtr->g_score = tentative_gScore;
                        neighborPtr->f_score = tentative_gScore + getHeu(neighborPtr, end_node);
                    }
                
            }
        }
        ros::Time time_2 = ros::Time::now();
        if ((time_2 - time_1).toSec() > 0.2)
        {
            ROS_WARN("Failed in A star path searching !!! 0.2 seconds time limit exceeded.");
			delete end_node;
            return false;
        }
	}

    delete end_node;

	if(a_star_gridpath.size()==0)
	{
		ROS_INFO_THROTTLE(1,"\033[1;%dm id: %d ,A STAR Can't Find a Path \033[0m",color_id,agent_id);
		return false;
	}
    ros::Time time_2 = ros::Time::now();
    ROS_INFO("\033[1;%dm id %d A_star spend %f, iter=%d start_pos: %.2f %.2f %.2f, local_goal: %.2f %.2f %.2f \033[0m",color_id,agent_id, 
							(time_2 - time_1).toSec(), num_iter,start_p(0),start_p(1),start_p(2),local_goal(0),local_goal(1),local_goal(2)
							);
    return true;

}
